package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Dashboard;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Skeleton;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.image.CriteriaCollection;


public class RobotMain extends SimpleRobot {
    
    //pwm ids for drive motor control
    private int fL = 4; //pwm id for front left motor
    private int fR = 3; //pwm id for front right motor
    private int rL = 2; //pwm id for rear left motor
    private int rR = 1; //pwm id for rear right motor
    private int shoot = 5; //pwm id for the shooter
    private int launcher = 1; //launcher port id 
    
    //Control declaration
    private Joystick leftJoystick;
    private Joystick rightJoystick;
    private Joystick gamePad;
    
    //Camera declarations
    private AxisCamera camera; 
    private CriteriaCollection collection;	//criteria for analyzing image
    
    //Dashboard update criteria declarations
    private Dashboard dashHigh;
    private Dashboard dashLow;
    
    RobotDrive drive; 
    Kinect autoDrive;
    
    Shooter shooter;
    private double shooterSpeed;
    
    
    public void robotInit()
    {
        drive = new RobotDrive(fL, fR, rL, rR);
        leftJoystick = new Joystick(2);
        rightJoystick = new Joystick(1);
        gamePad = new Joystick(3);
        
        shooterSpeed = 1;
        shooter = new Shooter(shoot, shooterSpeed, launcher);
        autoDrive = new Kinect(1,2);
        
        camera = AxisCamera.getInstance("10.17.45.11");
        camera.writeResolution(AxisCamera.ResolutionT.k160x120);
        camera.writeMaxFPS(15);
        camera.writeCompression(20);
        camera.writeColorLevel(50);
        camera.writeBrightness(40);
        collection = new CriteriaCollection();
    }
    
    public void autonomous() 
    {
        System.err.println("Entering autonomous:");
         while (true) 
         {
            Camera.imageGrab(camera, collection);
            
         }
    }
    
    public void operatorControl() 
    {
        System.err.println("Entering teleop:");
        while(true)
        {
            drive.tankDrive (-leftJoystick.getY(), -rightJoystick.getY());
            if(gamePad.getRawButton(1))
            {
                shooterSpeed = shooterSpeed -0.05;
            }
            if(gamePad.getRawButton(2))
            {
                shooterSpeed = shooterSpeed +0.05;
            }
        }
        
    }
   
   public void test()
   {
       System.err.println("Entering Test:");
       while(isEnabled())
       {   
           //if(/*autoDrive.getInstance().getSkeleton().GetTrackState() == Skeleton.tTrackState.kTracked && */autoDrive.getDeadZone()) 
            //{
               drive.tankDrive(-autoDrive.getLeftY(), -autoDrive.getRightY());
            
            //}
            //else
            //{
            //    drive.tankDrive(0,0);
            //    Timer.delay(.01);
            //}
       }
   }
}